import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;

/*
 * @author Okan Aşık
 * 
 * A simple algorithm to follow a wall of an obstacle
 * there will two simple behavior:
 * 1. going straight to a direction
 * 2. circumventing the obstacle
 * 
 */

public class WallFollowing {
	
	static TachoPilot robot = new TachoPilot(2.17f,4.57f,Motor.A, Motor.C);
	static TouchSensor touchSensor = new TouchSensor(SensorPort.S4);
	static UltrasonicSensor uSensor = new UltrasonicSensor(SensorPort.S1);

	public static void main(String[] args) {
		
		Motor.A.setSpeed(200);
	    Motor.C.setSpeed(200);
	    
	    Behavior gotoGoal = new GoToGoal();
	    Behavior followWall = new FollowWall();
	    
	    Behavior[] behaviorList =
	    {
	    		gotoGoal, followWall
	    };
	    
	    Arbitrator arbitrator = new Arbitrator(behaviorList);
	    LCD.drawString("Wall Following", 0, 0);
	    Button.waitForPress();
	    arbitrator.start();
	}
}

class GoToGoal implements Behavior {

	@Override
	public void action() {
		LCD.drawString("Going to Goal...", 0, 1);
		WallFollowing.robot.forward();
	}

	@Override
	public void suppress() {
		WallFollowing.robot.stop();
	}

	@Override
	public boolean takeControl() {
		return true;
	}
}

class FollowWall implements Behavior {

	private int backoff = 3;
	private int delta = 5;
	private int distance = 0;
	private int threshold = 3;
	private int corner = 20;
	
	@Override
	public void action() {
		
		LCD.drawString("Avoiding Obstacle.....", 0, 1);
		WallFollowing.robot.stop();
		//to ease the backing off
		try {
			Thread.sleep(1000);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
		}
		//back off for 10 centimeters
		WallFollowing.robot.travel((-1)*backoff);
		
		//align robot against the obstacle wall
		for (int i=0; i<720;  ) {
			  
			  int variance = 0;
			  distance = WallFollowing.uSensor.getDistance();
			  
			  try {
				  WallFollowing.robot.rotate(delta);
				  Thread.sleep(500);
				  i += delta;
			  }
			  catch (InterruptedException ex) {
			  }
			  
			  variance = distance - WallFollowing.uSensor.getDistance();
			  
			  //if there is no difference between read value from ultrasonic sensor than we are turned to correct position
			  if (variance  < 0) {
				  
				  //set total turn of the robot
				  WallFollowing.robot.rotate(-(2*delta));
				  break;
			  }
		  }
		
		while (true) {
			
			if (WallFollowing.uSensor.getDistance() >= corner + distance) {
				//turn the corner
				WallFollowing.robot.arc(-5, 90);
			}
			  
			  LCD.drawString("In an infinite loop", 0, 4);
			  
			  while (WallFollowing.uSensor.getDistance() > distance + threshold) {
				  WallFollowing.robot.rotate((-1)*delta);
			  }
			  
			  while (WallFollowing.uSensor.getDistance() < distance - threshold ) {
				  WallFollowing.robot.rotate(delta);
			  }
			  
			  while (WallFollowing.uSensor.getDistance() <= distance + threshold && WallFollowing.uSensor.getDistance() >= distance - threshold) {
				  WallFollowing.robot.travel(delta);
			  }
			  
		  }
		
	}

	@Override
	public void suppress() {
		// TODO Auto-generated method stub
	}

	@Override
	public boolean takeControl() {
		return WallFollowing.touchSensor.isPressed();
	}
	
}
